home *** CD-ROM | disk | FTP | other *** search
/ Aminet 6 / Aminet 6 - June 1995.iso / Aminet / misc / sci / RARS_Amiga_2.lha / RARS / cntrl5.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  1995-03-31  |  5.8 KB  |  137 lines
  1. // CNTRL5.CPP - "driver" function for RARS - M. Timin, Dec. 1994
  2. // adapted to ver. 0.39 3/6/95 by M. Timin
  3.  
  4. #include <string.h>
  5. #include <stdlib.h>
  6. #include <math.h>
  7. #include "car.h"
  8.  
  9. extern char* glob_name;       // The name string, below, will be copied here
  10. extern const double CARWID;   // car width, feet
  11.  
  12. // The task of this function is to compute vc and alpha.  To understand
  13. // vc and alpha, remember that in this model, the wheels are always slipping,
  14. // at least a little.  (Think of a cinder track.)  vc is the speed
  15. // of the bottom of the wheel relative to the car. (Velocity Commanded)
  16. // If the car is coasting at zero power then vc == s.v, the speed of the
  17. // car.  If vc is held constant for a while the cars speed will approach
  18. // vc, unless it is cornering, in which case vc may greatly exceed the 
  19. // speed.  The angle "alpha" is the angle between the car's orientation angle
  20. // and its velocity vector.  (like angle of attack of an aircraft)
  21. // Cornering force depends on alpha.  (It is also thought of as a slip angle.)
  22. // (alpha is not directly related to the steering wheel angle, which is not
  23. //  modeled here, and is assumed to rapidly oscillate about alpha to keep the
  24. //  car under control, and acheive the desired alpha.)
  25. con_vec cntrl5(situation s)
  26. {
  27.    const char name[] = "Stroust";      // This is the robot driver's name!
  28.    static int init_flag = 1;           // cleared by first call
  29.    // A "lane" is just a preferred distance from the wall of the track.
  30.    double width;                      // track width, feet
  31.    static double lane = 90;    // determines right-left position on straightaway
  32.    static int lane_time = 0;           // counts time when not in normal lane
  33.    con_vec result;                     // This is what is returned.
  34.    const double normalane = 0.0;      // usual position down the straight
  35.    const double steer_gain = .18;      // servo parameters on the straight:
  36.    const double steer_damp = .3;
  37.    static double ex_pwr_count = 0;    // counts calls when power_req > 1
  38.    double corn_speed;                 // target speed for cornering, ft/sec
  39.    double corn_next;                  // target speed for next corner
  40.    const double std_corner = 58.0;    // cornering speed for 100 ft. radius turn
  41.    double alpha, vc;           // components of result
  42.  
  43.    if(init_flag)  {            // first time through, only copy name:
  44.       strcpy(glob_name, name);
  45.       init_flag = 0;
  46.       result.alpha = result.vc = 0;
  47.       return result;
  48.    }
  49.  
  50.   if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
  51.       return result;
  52.  
  53.    width = s.to_lft + s.to_rgt;                        // find width of track
  54.    // compute a cornering speed proportional to square root of current radius:
  55.    if(s.cur_rad > 0.0)
  56.       corn_speed = std_corner * sqrt((s.cur_rad+.5*width)/150.0);
  57.    else if(s.cur_rad < 0.0)
  58.       corn_speed = std_corner * sqrt((.5*width-s.cur_rad)/150.0);
  59.    else corn_speed = 2 * std_corner;
  60.  
  61.    // compute a cornering speed for the next corner:
  62.    if(s.nex_rad > 0.0)
  63.       corn_next = std_corner * sqrt((s.nex_rad+.5*width)/150.0);
  64.    else if(s.nex_rad < 0.0)
  65.       corn_next = std_corner * sqrt((.5*width-s.nex_rad)/150.0);
  66.    else corn_next = 2 * std_corner;
  67.  
  68.    // maybe choose a different lane: (to help in passing)
  69.    if(!s.dead_ahead) {
  70.       if(lane_time <= 0) {
  71.          lane = normalane;
  72.          lane_time = 0;
  73.       }
  74.    }
  75.    else if(!lane_time)   {
  76.       // pick a different lane:
  77.       if(lane >= 80)               // pick a new lane somehow:
  78.          lane = random(60) - 20;
  79.       else if(lane <= -80)
  80.          lane = 20 - random(60);
  81.       else
  82.          lane = random(180) - 90;
  83.       lane_time = 160;              // we will stay in the new lane this long
  84.    }
  85.    if(lane_time > 0)
  86.       --lane_time;
  87.  
  88.    // set alpha based on a servo-mechanism approach, trying to maintain
  89.    // a certain distance from the track wall:
  90.    if(s.cur_rad == 0)  {     // If we are on the straightaway:
  91.       // alpha is proportional to lane error:
  92.       alpha = .35 * steer_gain * (s.to_lft - s.to_rgt - lane) / width;
  93.       if(s.dead_ahead)       // Change lanes quicker if someone in front:
  94.           alpha *= 2.0;
  95.    }
  96.    else if(s.cur_rad > 0) {  // in the turn, stick close to the inside:
  97.       alpha = steer_gain * (3 * (s.to_lft-s.to_rgt) / width);
  98.       if(s.dead_ahead)    // if someone ahead, reduce alpha to pass on outside
  99.           alpha *= .8;
  100.    }
  101.    else {                   // this is for right turns, similar to above
  102.       alpha = -steer_gain * (3 * (s.to_rgt-s.to_lft) / width);
  103.       if(s.dead_ahead)
  104.           alpha *= .8;
  105.    }
  106.    alpha -= steer_damp * s.vn / s.v;  // This is damping, to prevent oscillation
  107.  
  108.    // If we are on a straightaway:
  109.    if(s.cur_rad == 0 || s.cur_rad < -600.0 || s.cur_rad > 600.0)
  110.       if(s.to_end/s.cur_len > .28)      // if we are far from the end:
  111.          vc = s.v + 90/s.v;        // keep accellerating near full power
  112.       else  {                 // otherwise,
  113.          vc = (corn_next + 4.0 * s.v)/5.0;  // ready for next corner
  114.          if(s.dead_ahead)    // cut  if someone's dead ahead!
  115.              alpha *= 2.0;
  116.       }
  117.    else     {       // If we're in the curve, maintain speed.
  118.       if(s.to_end > .7)
  119.          vc = (corn_speed + 3.0 * s.v) / 4.0;
  120.       else
  121.          vc = (.5 * (corn_speed+corn_next) + 3 * s.v) / 4.0;
  122.    }
  123.  
  124.    // If last time we requested more power than is available, make
  125.    // vc closer to s.v to reduce requested power
  126.    if(s.power_req > 1.0)
  127.       ++ex_pwr_count;        // count how many times we want too much power
  128.    else if(ex_pwr_count > 0) // we decrement this to zero if s.power_req < 1
  129.      --ex_pwr_count;
  130.    if(ex_pwr_count > 0)
  131.       // Compute a weighted average of vc (as computed above) and s.v:
  132.       vc = (ex_pwr_count * s.v + 2 * vc)/(2 + ex_pwr_count);
  133.  
  134.    result.vc = vc;   result.alpha = alpha;
  135.    return result;
  136. }
  137.